import rclpy
from rclpy.node import Node
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionClient
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

class TrajectoryFeedback(Node):

    def __init__(self):
        super().__init__('trajectory_feedback')
        self.action_client = ActionClient(self, FollowJointTrajectory, '/arm_controller/follow_joint_trajectory')

    def send_goal(self):
        # 创建一个轨迹目标
        goal_msg = FollowJointTrajectory.Goal()
        goal_msg.trajectory = JointTrajectory()
        goal_msg.trajectory.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'r_joint']

        point = JointTrajectoryPoint()
        point.positions = [0.5, 0.5, 0.0, -0.5, 0.0, 0.0]
        point.time_from_start = rclpy.time.Duration(seconds=5.0).to_msg()

        goal_msg.trajectory.points = [point]

        # 发送目标
        self.action_client.wait_for_server()
        self.get_logger().info("Sending goal to arm controller...")
        send_goal_future = self.action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
        send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal was rejected by the action server.')
            return

        self.get_logger().info('Goal accepted by the action server.')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f'Goal result: {result}')

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f'Received feedback: {feedback}')

def main(args=None):
    rclpy.init(args=args)
    node = TrajectoryFeedback()
    node.send_goal()  # 发送目标
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
